#include "global.h"

/**
 * @brief 重置PID控制器对象的所有状态
 *
 * 将PID控制器的目标值、测量值、误差、上一次误差、积分项和输出值全部重置为0.0。
 *
 * @param pPid 指向PID控制器对象的指针
 */
void pid_rest(_st_PidObject *pPid)
{
    pPid->target = 0.0;     // 目标值
    pPid->measured = 0.0;   // 测量值
    pPid->error = 0.0;      // 当前误差
    pPid->errorPre = 0.0;   // 上一次误差
    pPid->errorInteg = 0.0; // 积分项
    pPid->out = 0.0;        // 输出值
}

/**
 * @brief 更新PID控制器的状态
 *
 * 根据当前目标值和测量值计算误差，并更新PID控制器的输出值。
 * 包括比例项、积分项和微分项的计算。
 *
 * @param pPid 指向PID控制器对象的指针
 * @param dt 时间步长（单位：秒）
 */
void pid_update(_st_PidObject *pPid, const float dt)
{
    float deriv; // 微分项

    // 计算误差
    pPid->error = pPid->target - pPid->measured;

    // 计算积分项
    pPid->errorInteg += pPid->error * dt;

    // 计算微分项
    deriv = (pPid->error - pPid->errorPre) / dt;

    // 计算PID输出
    pPid->out = pPid->kp * pPid->error + pPid->ki * pPid->errorInteg + pPid->kd * deriv;

    // 更新上一次误差
    pPid->errorPre = pPid->error;
}

/**
 * @brief 更新PID控制器的状态（直接基于误差版）
 *
 * 与`pid_update`类似，但不包含误差计算部分。假设误差已经在外部计算完成并存储在`pPid->error`中。
 * 包括积分项和微分项的计算，并更新PID控制器的输出值。
 *
 * @param pPid 指向PID控制器对象的指针
 * @param dt 时间步长（单位：秒）
 */
void pid_update_E(_st_PidObject *pPid, const float dt)
{
    float deriv; // 微分项

    // 计算积分项
    pPid->errorInteg += pPid->error * dt;

    // 计算微分项
    deriv = (pPid->error - pPid->errorPre) / dt;

    // 计算PID输出
    pPid->out = pPid->kp * pPid->error + pPid->ki * pPid->errorInteg + pPid->kd * deriv;

    // 更新上一次误差
    pPid->errorPre = pPid->error;
}
